classdef CodeTrackingLoop < matlab.System
    % 码跟踪环路

    % 这一部分结合原来HelperCodeCorrelator.m中的码相关器件
    % 自行补充 鉴别器 和 环路滤波器等 从而补全整个码环

    properties(Nontunable)
        PRNID = 1               % 卫星伪随机码序列编号
        SampleRate = 36.828e6   % 采样率，单位为 Hz
        IntegTime = 1e-3        % 积分时间，单位为秒
        codeRate = 1.023e6      % 码速率（即C/A码每秒码片数），单位为Hz
        codeLen = 1023;         % C/A码长度
        DLLFilter               % DLL环路滤波器
        discriminatorToZero     % 码鉴别器（Early-Late）输出是否为0
    end

    properties(Access = private)
        pNumCBPerInteg          % 每个积分区间内包含的码块数
        pPromptCode             % Prompt（准）码
        pPromptCodeEarly        % Early（提前）码
        pPromptCodeLate         % Late（延后）码
    end

    methods(Access = public)
        % 构造函数，初始化配置参数
        function obj = CodeTrackingLoop(signalConfig, rxConfig)
            obj.PRNID = signalConfig.signal.prnid;                  % 卫星PRN号
            obj.SampleRate = signalConfig.signal.SampleRate;        % 信号采样率
            obj.IntegTime = rxConfig.tracker.totalIntegTime;           % 积分时间 (可能有问题)
            obj.codeRate = signalConfig.signal.codeRate;  % GPS C/A码速率
            obj.codeLen = signalConfig.signal.codeLen;    % GPS C/A码长度
            obj.discriminatorToZero = rxConfig.tracker.dll.discriminatorToZero;

            obj.DLLFilter = ...
                HelperLoopFilter(rxConfig.tracker.dll.noiseBandwidth, ...
                rxConfig.tracker.dll.order, rxConfig.tracker.dll.flagBoxCarInteg);
        end

        % 环路滤波器
        function cphdelta = loopFilterImpl(obj, cpherr, steptime)
            cphdelta = obj.DLLFilter(cpherr, steptime);
        end

        % 码环 Early-Late 鉴别器 (单天线版本，输入已合并多天线)
        function [delayerr, cpherr] = discriminatorEL(obj, integ_coh_e_cob, integ_coh_l_cob, numCBPerInteg)
            % 码环 Early-Late 鉴别器 (单天线版本，输入已合并多天线)
            % 输入:
            %   iq_e - Early支路相关输出 (复数向量)
            %   iq_l - Late支路相关输出 (复数向量)
            %   numCBPerInteg - 积分区间内的码块数
            % 输出:
            %   delayerr - 延迟误差 (单位：码片)
            %   cpherr - 实际码相位误差

            if obj.discriminatorToZero % 码鉴别器输出为0，直接返回
                delayerr = 0;
                cpherr = 0;
                return;
            end

            % fixme: 只适合 相干积分时间内恰好有整数个码块的情况
            if numCBPerInteg == 1
                integ_coh_e_cob = sum(integ_coh_e_cob);
                integ_coh_l_cob = sum(integ_coh_l_cob);
            else
                integ_coh_e_cob = reshape(integ_coh_e_cob, [], numCBPerInteg);
                integ_coh_l_cob = reshape(integ_coh_l_cob, [], numCBPerInteg);
                integ_coh_e_cob = sum(integ_coh_e_cob, 2);
                integ_coh_l_cob = sum(integ_coh_l_cob, 2);
            end
            E = sum(abs(integ_coh_e_cob), 1);  
            L = sum(abs(integ_coh_l_cob), 1);  
            cpherr = (E - L) / (2 * (E + L));
            delayerr = -cpherr;
        end
    end

    methods(Access = protected)
        function setupImpl(obj)
            % 计算每个积分区间所需的码块数
            obj.pNumCBPerInteg = round(obj.IntegTime * obj.codeRate / obj.codeLen);

            % 生成对应PRN的C/A码，并进行BPSK调制（0→1, 1→-1），长度为1023×1
            code = 1 - 2 * double(gnssCACode(obj.PRNID, "GPS"));
%            code = ones(1023, 1);

            % 计算每个码片包含的采样点数
            numSamplesPerChip = obj.SampleRate / obj.codeRate;
            % 得到上采样与下采样因子，以保证能整除
            [upSampleFactor, downSampleFactor] = rat(numSamplesPerChip);

            % 对C/A码进行上采样，使其与采样率匹配
            upwave1 = repelem(code(:), upSampleFactor, 1);

            % 按积分区间长度生成CA码并下采样，得到准码
            obj.pPromptCode = repmat(upwave1(1 : downSampleFactor : end), obj.pNumCBPerInteg, 1);

            % 计算半个码片所对应的采样点，并生成“提前码”与“延后码”
            numSamplesPerHalfChip = round(numSamplesPerChip / 2);
            obj.pPromptCodeEarly = circshift(obj.pPromptCode, -numSamplesPerHalfChip);
            obj.pPromptCodeLate  = circshift(obj.pPromptCode, numSamplesPerHalfChip);
        end

        % 相关计算主函数，对输入信号与三种码（Early, Prompt, Late）做点乘
        function [iq_p, iq_e, iq_l] = stepImpl(obj, iqsig)
            iq_e = iqsig .* obj.pPromptCodeEarly;   % 提前相关输出
            iq_p = iqsig .* obj.pPromptCode;        % 准相关输出
            iq_l = iqsig .* obj.pPromptCodeLate;    % 延后相关输出
        end

        function resetImpl(obj)
            obj.DLLFilter.reset();
        end
    end
end
